
import math



'''
第二题：完成下一页基于A*算法的搜索题目
'''

# 地图大小
MAP_WIDTH = 6
MAP_HEIGHT = 6

# 障碍物坐标
OBSTACLES = [(3, 3), (3, 4), (3, 5)]

# 代价值
MOVE_COST = 1
DIAGONAL_COST = 1.4

# 起点和终点
START = (1, 3)
GOAL = (4, 4)


# 启发函数，计算当前节点到终点的距离
def heuristic(node):
    x, y = node
    goal_x, goal_y = GOAL
    return math.sqrt((goal_x - x) ** 2 + (goal_y - y) ** 2)


# 计算从起点到当前节点的实际代价
def get_cost(current_node, neighbor_node):
    # 如果当前节点和邻居节点在同一行或同一列，代价为1
    if current_node[0] == neighbor_node[0] or current_node[1] == neighbor_node[1]:
        return MOVE_COST
    # 否则代价为1.4
    else:
        return DIAGONAL_COST


# 获取当前节点的邻居节点
def get_neighbors(node, obstacles):
    x, y = node
    neighbors = set()
    for i in [-1, 0, 1]:
        for j in [-1, 0, 1]:
            # 如果是当前节点自身，或者超出地图边界，或者是障碍物，则跳过
            if (i == 0 and j == 0) or x + i < 0 or x + i >= MAP_WIDTH or y + j < 0 or y + j >= MAP_HEIGHT or (
                    x + i, y + j) in obstacles:
                continue
            neighbors.add((x + i, y + j))
    return neighbors


# A*算法搜索路径
def a_star_search(start, goal, obstacles):
    # 初始化起点
    open_set = {start}
    closed_set = set()
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start)}

    # 开始搜索
    while open_set:
        current = min(open_set, key=lambda node: f_score[node])
        if current == goal:
            # 找到了最短路径，返回路径
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            path.reverse()
            return path

        open_set.remove(current)
        closed_set.add(current)

        for neighbor in get_neighbors(current, obstacles):
            if neighbor in closed_set:
                continue

            tentative_g_score = g_score[current] + get_cost(current, neighbor)

            if neighbor not in open_set or tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = tentative_g_score + heuristic(neighbor)
                if neighbor not in open_set:
                    open_set.add(neighbor)

    # 没有找到最短路径，返回None
    return None


if __name__ == '__main__':
    # 执行A*算法搜索
    path = a_star_search(START, GOAL, OBSTACLES)
    print(path)
